
#include <SimpleFOC.h>
#include "user_config.h"



// BLDC motor & driver instance
BLDCMotor motor1 = BLDCMotor(MOTOR0_POLE_PAIR_NUM);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M0_INIT);
// MagneticSensorAnalog sensor1 = MagneticSensorAnalog(A7, 0, 1023); //模拟方式
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);



// BLDC motor & driver instance
BLDCMotor motor2 = BLDCMotor(MOTOR1_POLE_PAIR_NUM);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_INIT);
//MagneticSensorAnalog sensor2 = MagneticSensorAnalog(A6, 0, 1023); //模拟方式
MagneticSensorI2C sensor2 = MagneticSensorI2C(MT6701_I2C);


// angle set point variable
float target1_angle = 0;
float target2_angle = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget1(char* cmd) { command.scalar(&target1_angle, cmd); }
void doTarget2(char* cmd) { command.scalar(&target2_angle, cmd); }

void config_motor_control();

void setup() {

  // initialise magnetic sensor hardware
  sensor1.init();
  sensor2.init();  
  motor1.linkSensor(&sensor1);// link the motor to the sensor
  motor2.linkSensor(&sensor2);

  // driver config
  // power supply voltage [V]
  driver1.voltage_power_supply = BLDC_VOLTAGE_SUPPLY;
  driver1.init();
  motor1.linkDriver(&driver1);// link driver
  
  // power supply voltage [V]
  driver2.voltage_power_supply = BLDC_VOLTAGE_SUPPLY;
  driver2.init();
  motor2.linkDriver(&driver2);// link driver


  //config motor attributes
  config_motor_control();
	
  // use monitoring with serial
  Serial.begin(115200);
  // comment out if not needed
  motor1.useMonitoring(Serial);
  motor2.useMonitoring(Serial);


  // initialize motor
  motor1.init();
  motor2.init();  
  // align sensor and start FOC
  motor1.initFOC();
  motor2.initFOC();


  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target angle using serial terminal:"));
  _delay(1000);

   // 设置目标位置（3圈 = 3 * 2π ≈ 18.85弧度）
  target1_angle = 6 * _PI; // 注意：3圈实际是6π弧度（机械角度）
}


void loop() {

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz
  motor1.loopFOC();
  motor2.loopFOC();

  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code
  motor1.move(target1_angle);
  motor2.move(target2_angle);

   // 打印实际位置（可选）
  Serial.print("当前位置: ");
  Serial.println(motor1.shaft_angle);

   if (fabs(motor1.shaft_angle - target1_angle) < 0.1) {
    Serial.println("到达目标位置！");
    //while(1); // 停止程序
    
    target1_angle = -target1_angle;
    delay(1000);
  }



  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  // motor.monitor();

  // user communication
  command.run();
}





#define MOTOR_VOLTAGE_LIMIT   (3)//此时，单电机1.3A左右
//#define MOTOR_VOLTAGE_LIMIT (4)//此时，单电机0.4A左右
//#define MOTOR_VOLTAGE_LIMIT   (6)//此时，单电机0.75左右
//#define MOTOR_VOLTAGE_LIMIT   (8)//此时，单电机1.1-1.3A左右

/*
@func:函数：电机控制相关配置，包括：
a. 所处的模式：角度闭环还是速度闭环
b. 闭环中的PID参数
c. 闭环中的安全参数:voltage_limit 
*/
void config_motor_control()
{
	  // choose FOC modulation (optional)
  motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor2.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // set motion control loop to be used
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
  
	  // contoller configuration
  // default parameters in defaults.h

  // default voltage_limit
  motor1.voltage_limit = MOTOR_VOLTAGE_LIMIT; //12V情况下，最大为6.9V，防止电流过大；
  motor2.voltage_limit = MOTOR_VOLTAGE_LIMIT;//12V情况下，最大为6.9V，防止电流过大；


    // maximal velocity of the position control
  //motor1.velocity_limit = 20;
  motor1.velocity_limit = 0.8;
  motor2.velocity_limit = 20;  


  //Angle PID 
  motor1.P_angle.P = 0.5;
  motor1.P_angle.I = 0.2;         // 增加位置积分
  motor1.P_angle.D = 0.005;       // 增加位置微分

  //velcocity PID
  motor1.PID_velocity.P = 0.2f;
  motor1.PID_velocity.I = 4;
  motor1.PID_velocity.D = 0;


  // contoller configuration based on the controll type
  motor2.PID_velocity.P = 0.1f;
  motor2.PID_velocity.I = 2;
  motor2.PID_velocity.D = 0;

  
  // velocity low pass filtering time constant
  // the lower the less filtered
  motor1.LPF_velocity.Tf = 0.05f;
  motor2.LPF_velocity.Tf = 0.05f;

}




